/*****************************************************************
**          大连理工大学 凌BUG战队&大连民族大学 C.ONE战队
**                **生死有命，富贵在天**
**    没有什么难题是一个通宵解决不了的，如果有那就整两个！
**---------------------------------------------------------------
** 项目名称：   dlmu-hero-robot
** 日    期：   2024-03-12
** 作    者：   MasterWang & Changed by TuxMonkey
**---------------------------------------------------------------
** 文 件 名：   motor_control.h
** 文件说明：   机器人电机、超级电容控制和运动解算
*****************************************************************/
#ifndef MOTOR_CONTROL_H
#define MOTOR_CONTROL_H

/*---------------------INCLUDES----------------------*/
#include "main.h"
#include "remote_control.h"

/*---------------------DEFINES-----------------------*/
//
#define CHASSIS_MOTOR_MAXSPEED				8500		//底盘电机单独能达到的最大速度 rpm

#define YAW6020_FORWARD_POSITION			1200		//机器人枪管对正底盘前方时yaw6020的角度参数
#define YAW6020_45DEGREE_POSITION			13//机器人枪管水平逆时针转45度时yaw6020的角度参数
#define PIT6020_HORIZONTAL_POSITION		4096		//机器人枪管水平时pitch6020的角度参数

#define BULLETROOM_COVER_OPEN				550			//弹舱盖舵机打开值
#define BULLETROOM_COVER_CLOSE			1815		//弹舱盖舵机关闭值

/*----------------------ENUMS------------------------*/
//CAN send and receive ID
typedef enum
{
    //
    CAN_MOTOR_CTRL_ID_1_4 = 0x200,
    CAN_MOTOR_CTRL_ID_5_8 = 0x1FF,
    CAN_MOTOR_CTRL_ID_9_C = 0x2FF, 

    CAN_3508_MOTOR1_ID = 0x201, //chassis can1_fifo0
    CAN_3508_MOTOR2_ID = 0x202,
    CAN_3508_MOTOR3_ID = 0x203,
    CAN_3508_MOTOR4_ID = 0x204,

	CAN_3508_TURNBULLET_ID = 0x206, //ammobooster_turnbullet can1_fifo1
//	CAN_3508_TURNBULLET_ID = 0x205, //ammobooster_turnbullet can1_fifo1
	  
//	CAN_6020_YAW_ID = 0x206, //gimbal_yaw =can1_fifo1 yaw 206
	CAN_6020_YAW_ID = 0x205, //gimbal_yaw =can1_fifo1 yaw 205
	
    CAN_6020_PIT_ID = 0x205, //gimbal_pitch ***can2*** _ fifo0 pitch 205

    CAN_3508_SHOOT1_ID = 0x206, //ammobooster_shoot can2_fifo0
    CAN_3508_SHOOT2_ID = 0x207,


    //
    CAN_SUPERCAP_STAT1_ID = 0x301 //super_cap can1_fifo1 100Hz

} can_msg_id;

/*---------------------STRUCTS-----------------------*/
//rm motor data
typedef struct
{
    uint16_t angle;
    int16_t speed_rpm;
    int16_t current;
    uint8_t temperature;

} Motor_Status_t;

//rm motor ex_data
typedef struct
{
    uint16_t offset_angle;
    uint16_t last_angle;
    int16_t delt_angle;
    int16_t round_cnt;
    int32_t total_angle;
    int32_t target_angle;

} Motor_Status_Ex_t;//关于当前电机数据，由结构体统一sss

//robot motor speed
typedef struct
{
    float chassis_speed[4];
    float yaw_speed;
    float pitch_speed;
    float shoot_speed;
    float turnbullet_speed;

} Robot_Motor_Speed_t;

/*---------------------DECLARES----------------------*/
//电机反馈状态数据
extern Motor_Status_t motor_status_chassis[4];
extern Motor_Status_t motor_status_gimbal[2];
extern Motor_Status_t motor_status_ammobooster[3];
extern Motor_Status_Ex_t motor_status_ex_turnbullet;

//机器人电机速度数据
extern Robot_Motor_Speed_t robot_motor_speed;

//超级电容反馈的信息
extern float supercap_voltage;//关于超级电容主要由电压电流两个方向来控制，因为电机特性其电压电流在整体的闭环特性下呈现2个阶段
extern float supercap_energy_percent;//由于电机系统内部ACR,ASR存在首先便是从初始的启动时，通过操作端控制电机时，电机转速需要一定时间调节，步兵运动本身也可以认为电机挂载了一个负载，

//底盘电机ID快速重置标志位
extern uint8_t chassis_reset_id_flag;

//电机快速重置ID
extern void CAN_cmd_chassis_reset_ID(void);

//发送控制底盘、云台、发射机构电机的CAN指令
extern void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4);
//extern void CAN_cmd_gimbal_ammobooster(int16_t pitch, int16_t yaw, int16_t shoot1, int16_t shoot2, int16_t turnbullet);
extern void CAN_cmd_gimbal_ammobooster(int16_t yaw, int16_t pitch, int16_t shoot1, int16_t shoot2, int16_t turnbullet);

//闭环控制底盘、云台、发射机构电机的速度
extern void set_chassis_motor_speed(float wheel_speed[]);
extern void set_gimbal_ammobooster_motor_speed(int16_t yaw_speed, int16_t pitch_speed, int16_t shoot_speed, int16_t turnbullet_speed);

//闭环控制云台角度
extern void set_gimbal_imu_angle(float yaw_angle, float pitch_angle);
extern void set_gimbal_6020_angle(float yaw_angle, float pitch_angle);

//机器人运动解算
extern void robot_motion_resolving(ROBOT_ctrl_t *robot_ctrl, Robot_Motor_Speed_t *robot_motor_speed);

//电机速度环调试
extern void robot_motor_speed_control_test(void);


#endif
